#include "Position3D.h"
#include <QDebug>
#include "support.h"

Position3D::Position3D(DepthInputFilter &depthInput, bool bigImageMode) : Filter(2, 1), depthInput(&depthInput), bigImage(bigImageMode)
{
}


Position3D::~Position3D(void)
{
}

void Position3D::Process()
{
	float factor;
	if (bigImage)
		factor = 0.5;
	else
		factor = 1.0;

	int pointsCount = inputs[0]->rows;

	cv::Mat projective(pointsCount, 3, CV_32FC1);

	for (int i = 0; i < pointsCount; ++i)
	{
		int *in = inputs[0]->ptr<int>(i);
		projective.at<float>(i, 0) = in[0] * factor;
		projective.at<float>(i, 1) = in[1] * factor;
		projective.at<float>(i, 2) = inputs[1]->at<ushort>(in[1] * factor, in[0] * factor);

	}

	outputs[0] = depthInput->ConvertProjectiveToRealWorld(projective);
}
